#include "viewer_2d_widget.h"
#include <QPainter>
#include <QTimer>
#include "mainwindow.h"
#include <fstream>

int COLORS_2D[16][3] = {
    { 0, 127, 255 },//deepskyblue
    { 0, 255, 0 },// green
    { 0, 0, 255 },// blue
    { 255, 255, 0 },// yellow
    { 0, 255, 255 },// cryan
    { 255, 0, 255 },//magenta
    { 255, 255, 255 },// white
    { 127, 0, 255 },// purple
    { 255, 0, 0 },// red
    { 127, 255, 0 },//greenyellow
    { 0, 255, 127 },//aqua
    { 255, 127, 0 },// orange
    { 255, 0, 127 },//AmericanBeauty
    {255, 127,127},// red
    {255, 127, 255},//Orchid
    {0, 127, 127}//deep green
};

#define SET_2D_COLOR(x) QColor(COLORS_2D[x][0],COLORS_2D[x][1],COLORS_2D[x][2])

enum ColorType{
  DEEPSKYBLUE,
  GREEN,
  BLUE,
  YELLOW,
  CRYAN,
  MAGENTA,
  WHITE,
  PURPLE,
  RED,
  GREENYELLOW,
  AQUA,
  ORANGE,
  AMERICANBEAUTY,
  SALMON,
  ORCHID,
  DEEP_GREEN
};


int yxRate = 1;
const int IVD_OMP = -1;  //无效的鼠标位置
using namespace std;

inline void BezierCtrlPts(QPointF p0, QPointF p1, QPointF p2, QPointF p3,
                          QPointF &u, QPointF &v) {
  const double tension = 0.5f;
  u = (p2 - p0) * (tension / 3) + p1;
  v = (p1 - p3) * (tension / 3) + p2;
}

void drawCurve(QPainter *painter, const vector<QPointF> &pts) {
  int n = pts.size();
  if (n < 2) return;
  if (n == 2) {
    painter->drawLine(pts[0].x(), pts[0].y(), pts[1].x(), pts[1].y());
  } else {
    QPainterPath path;
    path.moveTo(pts[0]);
    QPointF u, v;
    BezierCtrlPts(pts[0], pts[0], pts[1], pts[2], u, v);
    path.cubicTo(u, v, pts[1]);
    for (int i = 2; i < n - 1; i++) {
      BezierCtrlPts(pts[i - 2], pts[i - 1], pts[i], pts[i + 1], u, v);
      path.cubicTo(u, v, pts[i]);
    }
    BezierCtrlPts(pts[n - 3], pts[n - 2], pts[n - 1], pts[n - 1], u, v);
    path.cubicTo(u, v, pts[n - 1]);
    painter->setBrush(Qt::NoBrush);
    painter->drawPath(path);
  }
}

//![0]
Viewer2DWidget::Viewer2DWidget(MainWindow *parent): main_window_(parent)
{
  setMouseTracking(true);
  lnPen = QPen(QColor(250, 80, 40));
  lnPen.setWidth(2);
  obPen = QPen(QColor(164, 0, 164));  //紫色
  obPen.setWidth(2);
  scPen.setColor(QColor(100, 255, 100));
  scPen.setWidthF(1);
  textPen = QPen(Qt::yellow);
  textFont.setPixelSize(15);
  gridPen = QPen(Qt::white);
  gridPen.setWidthF(0.2);
  bhOb = QBrush(QColor(100, 120, 60));
  bhBd = QBrush(QColor(0, 120, 60));
  oxRate = 0.5;
  oyRate = 0.5;
  dRate = 10;
  mousePos_.x = 0;
  mousePos_.y = 0;
  traking_points_ = new   CycArray<STRoadPoint>(1000);
  map_point_smooth_process_ = new vehicle::map::MapPointSmoothProcess();

  oMousPos.setX(IVD_OMP);
  map_info_process_thread_.reset(new std::thread(&Viewer2DWidget::MapInfoProcess,this));
  map_info_process_thread_->detach();

  path_generater_thread_.reset(new std::thread(&Viewer2DWidget::PathGenerater,this));
  path_generater_thread_->detach();
}

Viewer2DWidget::~Viewer2DWidget()
{

    stop_thread_ = true;
    usleep(100000);
    if(map_info_process_thread_ && map_info_process_thread_->joinable())
    {
        map_info_process_thread_->join();
    }
    if(path_generater_thread_ && path_generater_thread_->joinable())
    {
        path_generater_thread_->join();
    }
    delete  traking_points_;
    delete  map_point_smooth_process_;
}

void Viewer2DWidget::closeEvent(QCloseEvent *event)
{
     QWidget::closeEvent(event);
}

void Viewer2DWidget::PathGenerater()   //生成map info路劲
{
    while(!stop_thread_)
    {
        usleep(50000);
        if(mouse_target_type_ == MouseTaskType::UNKNOWN)   //只有在不是编辑路点或者gps点时生成
        {
            SetofMapInfo set_of_map = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info();    //获取最匹配的区域路径。
            STRoadPoint ego_point = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
            if(ego_point.location_state == 0) //当gps信号不正常时
            {
                printf("gps location is not normal ,please check gps signal!!!\r\n");
                continue;
            }
            std::vector<STRoadPoint> match_gps_points;
            map_point_smooth_process_->MapPointSmoothInit(set_of_map.gps_points);   //对整个匹配路径做处理初始化
            int near_index;
            if(!map_point_smooth_process_->LoadLocalizeInSegmentMap(ego_point,match_gps_points,near_index,50,75))   //提取一段
            {
                printf("get section road faild,attemped get again !!!\r\n");
                continue;
            }
            std::map<int,float> near_index_dis_map;
            std::map<int,STRoadPoint> near_index_wps_map;
            for(int i=0;i<set_of_map.wps_points.size();i++)   //对wps点与gps点做一一匹配
            {
                int near_point_index;
                STRoadPoint best_point = FindBestMatchPointFromPoint(set_of_map.gps_points,set_of_map.wps_points.at(i),near_point_index,false);
                float wps_point_S = map_point_smooth_process_->getS(near_point_index);
                float gps_point_S = map_point_smooth_process_->getS(near_index);
                float dis = std::hypot(best_point.global_x - set_of_map.wps_points.at(i).global_x,best_point.global_y - set_of_map.wps_points.at(i).global_y);
               if(dis < 2)
               {
                    near_index_dis_map.insert(make_pair(near_point_index,wps_point_S-gps_point_S));
                    near_index_wps_map.insert((make_pair(near_point_index,set_of_map.wps_points[i])));
                    printf("index: [%d,%d],%f,%f\r\n",i,near_point_index,set_of_map.wps_points[i].left_bound,set_of_map.wps_points[i].right_bound);
               }
            }

            //对边界点参考线做提取
            std::vector<STRoadPoint> boundary_refrence_points;
            std::vector<int> index_vec;
            std::vector<lcmtypes::map_point_t> boundary_left_points;
            std::vector<lcmtypes::map_point_t> boundary_right_points;
            int km1 = near_index,kp1 = near_index;
            while(km1>=0)
            {
                if(map_point_smooth_process_->getS(near_index) - map_point_smooth_process_->getS(km1) > 8)
                    break;
                km1--;
            }
            km1 = std::max<int>(km1, 0);

            while(kp1 < set_of_map.gps_points.size())
            {
                if(map_point_smooth_process_->getS(kp1) - map_point_smooth_process_->getS(near_index) > 20)
                    break;
                kp1++;
            }

            boundary_refrence_points.clear();
            index_vec.clear();
            kp1 = std::min<int>(kp1, set_of_map.gps_points.size()-1);
            for (size_t i = 0; i < kp1 - km1 + 1; ++i) {
                boundary_refrence_points.push_back(set_of_map.gps_points.at(km1 + i));
                index_vec.push_back(km1 + i);
            }
//计算出边界点
            CalcRightAndLeftBoundaryPoints(boundary_refrence_points,index_vec,near_index_wps_map,near_index,boundary_left_points,boundary_right_points);

            lcmtypes::map_point_t map_point_l;
            lcmtypes::map_info_t map_info;
            map_info.boundaryCount = 0;
            map_info.specialPointCount = 0;
            for(int kk=0;kk<3;kk++)
            {
                map_info.frontLaneInfo[kk].pointCount = 0;
                map_info.backLaneInfo[kk].pointCount = 0;
                map_info.frontLaneInfo[kk].points.clear();
                map_info.backLaneInfo[kk].points.clear();
            }

            for(int kk=0;kk<4;kk++)
            {
                map_info.frontLineInfo[kk].pointCount = 0;
                map_info.backLineInfo[kk].pointCount = 0;
            }

            for(int kk=0;kk<(int)match_gps_points.size();kk++)
            {
                map_point_l.globalX = match_gps_points.at(kk).global_x;
                map_point_l.globalY = match_gps_points.at(kk).global_y;
                GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(map_point_l.globalX,map_point_l.globalY,&map_point_l.lat,&map_point_l.lon);
                STRoadPoint ego_position = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();

                double local_l[3],gloable_l[3],vehicle_l[3];
                vehicle_l[0] = ego_position.global_x;
                vehicle_l[1] = ego_position.global_y;
                vehicle_l[2] = ego_position.theta;

                gloable_l[0] = map_point_l.globalX;
                gloable_l[1] = map_point_l.globalY;
                GPSCoordinateTool::Instance()->Gloable2Relative(gloable_l,local_l,vehicle_l);
                map_point_l.localX = local_l[0];
                map_point_l.localY = local_l[1];

                //   map_point_l.lon = match_gps_points.at(kk).lon;
                //   map_point_l.lat = match_gps_points.at(kk).lat;
                //   map_point_l.localX = match_gps_points.at(kk).local_x;
                //   map_point_l.localY = match_gps_points.at(kk).local_y;
                map_info.frontLaneInfo[1].points.push_back(map_point_l);
            }

            map_info.boundaryCount = 2;
            map_info.boundary.resize(2);
            map_info.boundary[0].pointCount = boundary_left_points.size();
            map_info.boundary[0].points = boundary_left_points;

            map_info.boundary[1].pointCount = boundary_right_points.size();
            map_info.boundary[1].points = boundary_right_points;

            //	    printf("lane size %d\r\n",map_info.frontLaneInfo[1].points.size());
            auto next_wps_point = near_index_dis_map.lower_bound(near_index);
            if(next_wps_point != near_index_dis_map.end())
            {
                next_wps_point_ = near_index_wps_map[next_wps_point->first];
                map_info.speedLimited = next_wps_point_.speed_limit;
                map_info.distance2NextSegment = next_wps_point->second;
            }

            map_info.laneCount = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info().gps_points.at(near_index).lane_num;
            map_info.distance2LeftMarker = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info().gps_points.at(near_index).left_bound;
            map_info.distance2RightMarker = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info().gps_points.at(near_index).right_bound;
            map_info.turnType = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info().gps_points.at(near_index).tracing_mode;
            map_info.frontLaneInfo[1].pointCount = map_info.frontLaneInfo[1].points.size();
            vehicle::map::DataProviderMapInfo::Instance()->Update(map_info);
        }
    }
}


void Viewer2DWidget::CalcRightAndLeftBoundaryPoints(std::vector<STRoadPoint> boundary_refrence_points,std::vector<int> point_index_vec,std::map<int,STRoadPoint> wps_index_map,int near_index,std::vector<lcmtypes::map_point_t> &boundary_left_points,std::vector<lcmtypes::map_point_t> &boundary_right_points)
{
    boundary_left_points.clear();
    boundary_right_points.clear();
    std::vector<float> x;
    std::vector<float> y;
    std::vector<float> k;
    lcmtypes::map_point_t left_point;
    lcmtypes::map_point_t right_point;

    if(boundary_refrence_points.size()>0)
    {
        for(int kk=0;kk<boundary_refrence_points.size();kk++)
        {
            x.emplace_back(boundary_refrence_points.at(kk).global_x);
            y.emplace_back(boundary_refrence_points.at(kk).global_y);
        }
        double dx = x[1]-x[0];
        double dy = y[1]-y[0];
        k.emplace_back(std::atan2(dy, dx));
        for(int kk =1;kk<x.size();kk++)
        {
            double dx = x[kk] - x[kk-1];
            double dy = y[kk] - y[kk-1];
            k.emplace_back(std::atan2(dy, dx));
        }
    }

    for(int i=0 ;i<k.size();i++)
    {
        auto next_wps_point = wps_index_map.lower_bound(point_index_vec.at(i));
        right_point.globalX = x[i] + std::fabs(next_wps_point->second.right_bound)* std::sin(k[i]);
        right_point.globalY = y[i] - std::fabs(next_wps_point->second.right_bound)* std::cos(k[i]);

        GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(right_point.globalX,right_point.globalY,&right_point.lat,&right_point.lon);
        STRoadPoint ego_position = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();

        double local_l[3],gloable_l[3],vehicle_l[3];
        vehicle_l[0] = ego_position.global_x;
        vehicle_l[1] = ego_position.global_y;
        vehicle_l[2] = ego_position.theta;

        gloable_l[0] = right_point.globalX;
        gloable_l[1] = right_point.globalY;
        GPSCoordinateTool::Instance()->Gloable2Relative(gloable_l,local_l,vehicle_l);
        right_point.localX = local_l[0];
        right_point.localY = local_l[1];
        boundary_right_points.push_back(right_point);

        left_point.globalX = x[i] - std::fabs(next_wps_point->second.left_bound)* std::sin(k[i]);
        left_point.globalY = y[i] + std::fabs(next_wps_point->second.left_bound)* std::cos(k[i]);

        GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(left_point.globalX,left_point.globalY,&left_point.lat,&left_point.lon);
        gloable_l[0] = left_point.globalX;
        gloable_l[1] = left_point.globalY;
        GPSCoordinateTool::Instance()->Gloable2Relative(gloable_l,local_l,vehicle_l);
        left_point.localX = local_l[0];
        left_point.localY = local_l[1];
        boundary_left_points.push_back(left_point);
    }
}

//根据路劲中点群与单点最匹配的点
STRoadPoint Viewer2DWidget::FindBestMatchPointFromPoint(std::vector<STRoadPoint> &points, STRoadPoint point_info_l,int &best_index, bool yaw_flag)
{
    STRoadPoint best_point;
    float min_distance_inner = 10000;
    for(int i=0;i<points.size();i++)
    {
        if(yaw_flag == true)
        {
            if(fabs(points.at(i).yaw - point_info_l.yaw) <= M_PI/2)
            {
                float dis = std::hypot(points.at(i).global_x - point_info_l.global_x,points.at(i).global_y - point_info_l.global_y);
                min_distance_inner = dis < min_distance_inner?dis:min_distance_inner;
                if(dis < min_distance_inner)
                {
                    min_distance_inner = dis;
                    best_point = points.at(i);
                    best_index = i;
                }
            }
        }else{
            float dis = std::hypot(points.at(i).global_x - point_info_l.global_x,points.at(i).global_y - point_info_l.global_y);
            if(dis < min_distance_inner)
            {
                min_distance_inner = dis;
                best_point = points.at(i);
                best_index = i;
            }
        }
    }
    return best_point;
}

//匹配一群路劲中，最匹配的一个路径
SetofMapInfo Viewer2DWidget::FindBestMatchMapInfoFromPoint(std::map<int, SetofMapInfo> &map_info, STRoadPoint point_info_l, bool yaw_flag)
{
    std::map<int, SetofMapInfo> backup_map_info;
    SetofMapInfo select_map_info;
    backup_map_info.clear();

    if(map_info.size() == 1)
        return map_info.begin()->second;

    for(auto data: map_info)
    {
//        vehicle::common::math::Vec2d point_rt(data.second.right+5,data.second.top+5);
//        vehicle::common::math::Vec2d point_lb(data.second.left-5,data.second.bottom-5);
        ab::common::math::Box2d box2d = data.second.boundary_box;

        vehicle::common::math::Vec2d point(point_info_l.global_x,point_info_l.global_y);
        if(box2d.IsPointIn(point))
        {
            backup_map_info.insert(data);
        }

    }
    if(backup_map_info.size() > 0)
    {
        int best_index  = backup_map_info.begin()->first;
        float min_distance = 10000;
        for(auto backup : backup_map_info)
        {
            float min_distance_inner = 10000;
            for(int i=0;i<backup.second.gps_points.size();i++)
            {
                if(yaw_flag == true)
                {
                    if(fabs(backup.second.gps_points.at(i).yaw - point_info_l.yaw) <= M_PI/2)
                    {
                        float dis = std::hypot(backup.second.gps_points.at(i).global_x - point_info_l.global_x,backup.second.gps_points.at(i).global_y - point_info_l.global_y);
                        min_distance_inner = dis < min_distance_inner?dis:min_distance_inner;
                    }
                }else{
                    float dis = std::hypot(backup.second.gps_points.at(i).global_x - point_info_l.global_x,backup.second.gps_points.at(i).global_y - point_info_l.global_y);
                    min_distance_inner = dis < min_distance_inner?dis:min_distance_inner;
                }
            }
            if(min_distance_inner < min_distance)
            {
                best_index = backup.first;
                min_distance = min_distance_inner;
            }
        }
        select_map_info = backup_map_info[best_index];
    }
    return select_map_info;
}

//循环处理
void Viewer2DWidget::MapInfoProcess()
{
    std::map<int, SetofMapInfo> map_info;
    SetofMapInfo select_map_info;
    STRoadPoint last_point_info;

    while(!stop_thread_)
    {
        usleep(50000);
        {
            STRoadPoint point_info_l = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
            if(main_window_->traking_gps_flag_ == MouseTaskType::TRAKING_GPS_POINTS)
            {
                double point_info1[2],point_info2[2];
                point_info1[0] = point_info_l.global_x;
                point_info1[1] = point_info_l.global_y;
                point_info2[0] = last_point_info.global_x;
                point_info2[1] = last_point_info.global_y;
                if(GPSCoordinateTool::Instance()->DistancePoint2Point(point_info1,point_info2)> 1||(std::fabs(last_point_info.yaw-point_info_l.yaw)>15*M_PI/180))
                {
                    traking_points_->Update(point_info_l);
                    last_point_info = point_info_l;
                }
            }else
            {
                traking_points_->Clear();
            }

            //if(vehicle_status.nauto)
            //  continue;
            map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();
            if(map_info.size()<=0)
            {
                printf(" maps data empty,please record a map !!\r\n");
                continue;
            }
            select_map_info = FindBestMatchMapInfoFromPoint(map_info,point_info_l, true);
            vehicle::map::DataProviderSetofMapInfo::Instance()->Update(select_map_info);
        }
    }
}

void Viewer2DWidget::find_car_triggered()
{
    printf("find car.........\r\n");
    STRoadPoint point_info_l = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
    float dx1 = 0, dy1 = 0, dx2, dy2;
    // 定义矩形的左上角和右下角坐标
    dx1 = point_info_l.global_x * dRate;
    dy1 = point_info_l.global_y * dRate;
    OX = width()/2-dx1;
    OY = height()/2+dy1;
}

//打开地图文件夹触发回调,读取一个路径或者多个路径
void Viewer2DWidget::OpenMapFileListTriggered(const std::string &folder,std::vector<std::string> &file_list)
{
    std::map<int, SetofMapInfo> map_info;
    std::string sub_string = "gps.txt.";
    if(file_list.size() == 0)
        return;
    map_info.clear();
    boundary_boxs_.clear();
    for(auto file :file_list)
    {
        // 查找子字符串的位置
        size_t position = file.find(sub_string);
        if (position != std::string::npos) {
            // 截取子字符串之外的字符
            std::string result = file.substr(0, position) + file.substr(position + sub_string.length());
            if(result != "")
            {
                std::vector<STRoadPoint> gps_points;
                std::vector<STRoadPoint> wps_points;
                Box2d boundary_value;
                SetofMapInfo set_of_map_info;
                gps_points.clear();
                wps_points.clear();
                int index = std::stoi(result);
                boundary_value = GetMapDataFromFile(index,folder,gps_points,wps_points);
                if(gps_points.size() <= 1)   //if the points size equal 1,ignore.
                    continue;
                set_of_map_info.index = index;
//                set_of_map_info.top = boundary_value[0];
//                set_of_map_info.bottom = boundary_value[1];
//                set_of_map_info.left = boundary_value[2];
//                set_of_map_info.right = boundary_value[3];
                set_of_map_info.boundary_box = boundary_value;
                set_of_map_info.index = index;
                set_of_map_info.gps_points = gps_points;
                set_of_map_info.wps_points = wps_points;
                map_info.insert(std::make_pair(index,set_of_map_info));
            }
        }
    }
    if(map_info.size() > 0)
    {
        for(auto set_of_map:map_info)
        {
            if(set_of_map.second.gps_points.size() >= 2)
            {
                double dx = set_of_map.second.gps_points.at(1).global_x - set_of_map.second.gps_points.at(0).global_x;
                double dy = set_of_map.second.gps_points.at(1).global_y - set_of_map.second.gps_points.at(0).global_y;
                set_of_map.second.gps_points.at(0).theta = vehicle::tools::cast_from_0_to_2PI_Angle((std::atan2(dy, dx)));
                set_of_map.second.gps_points.at(0).yaw = vehicle::tools::cast_from_0_to_2PI_Angle(M_PI/2 - set_of_map.second.gps_points.at(0).theta);
                for(int i=1;i<set_of_map.second.gps_points.size();i++)
                {
                    double dx = set_of_map.second.gps_points.at(i).global_x - set_of_map.second.gps_points.at(i-1).global_x;;
                    double dy = set_of_map.second.gps_points.at(i).global_y - set_of_map.second.gps_points.at(i-1).global_y;
                    set_of_map.second.gps_points.at(i).theta = vehicle::tools::cast_from_0_to_2PI_Angle((std::atan2(dy, dx)));
                    set_of_map.second.gps_points.at(i).yaw = vehicle::tools::cast_from_0_to_2PI_Angle(M_PI/2 - set_of_map.second.gps_points.at(i).theta);
                }
            }
        }

        vehicle::map::DataProviderMapInfoPoints::Instance()->Update(map_info);
        initialization_ = true;
    }

}

Box2d Viewer2DWidget::GetMapDataFromFile(int index,std::string folder,std::vector<STRoadPoint> &gps_points,std::vector<STRoadPoint> &wps_points)
{
    std::vector<Vec2d> collect_points;
    std::string gps_sub_string = "/gps.txt.";
    std::string wps_sub_string = "/wps.txt.";
    float top=DBL_MIN,bottom = DBL_MAX,left = DBL_MAX,right = DBL_MIN;
    std::string open_file_name = folder+gps_sub_string + to_string(index);
    std::cout<<"load file:"<<open_file_name<<std::endl;
    if(vehicle::tools::fileExists(open_file_name))
    {
        gps_file_.open(open_file_name);
        if(gps_file_.is_open())
        {
            std::string line;
            std::getline(gps_file_, line);
            while(std::getline(gps_file_,line))
            {
                std::istringstream iss(line);
                std::string field;
                std::vector<std::string> fieldsInLine;
                STRoadPoint point;
                fieldsInLine.clear();

                while (std::getline(iss, field, '\t')) {
                    fieldsInLine.push_back(field);
                    //                          std::cout<<field<<"\t";
                }
             //   std::cout<<std::endl;
                if(fieldsInLine.size() != 6)
                    continue;
                point.lon = std::stod(fieldsInLine[0]);
                point.lat = std::stod(fieldsInLine[1]);
                point.global_x = std::stof(fieldsInLine[2]);
                point.global_y = std::stof(fieldsInLine[3]);
                Vec2d point_l(point.global_x,point.global_y);
                point.yaw = std::stof(fieldsInLine[4]);
//                left = left < point.global_x ? left : point.global_x;
//                right = right > point.global_x ? right : point.global_x;
//                top = top > point.global_y ? top : point.global_y;
//                bottom = bottom < point.global_y ? bottom : point.global_y;
                gps_points.push_back(point);
                collect_points.push_back(point_l);
           //     std::cout<<std::fixed << std::setprecision(7)<<point.lon<<" : "<<point.lat<<std::endl;
            }
        //    std::cout<<left-5<<" : "<<right+5<<" : "<<top+5<<" : "<<bottom-5<<std::endl;
        }
        gps_file_.close();
    }

    Polygon2d shape_points(collect_points);
    Box2d parallelogram_shape =  shape_points.MinAreaBoundingBox();
    parallelogram_shape.LongitudinalExtend(5);
    parallelogram_shape.LateralExtend(5);
    boundary_boxs_.push_back(parallelogram_shape);

    std::string open_wps_file_name = folder+wps_sub_string + to_string(index);
    std::cout<<"load wps file:"<<open_wps_file_name<<std::endl;
    if(vehicle::tools::fileExists(open_wps_file_name))
    {
        wps_file_.open(open_wps_file_name);
        if(wps_file_.is_open())
        {
            std::string line;
            std::getline(wps_file_, line);
            int index=0;
            while(std::getline(wps_file_,line))
            {
                std::istringstream iss(line);
                std::string field;
                std::vector<std::string> fieldsInLine;
                STRoadPoint point;
                fieldsInLine.clear();

                while (std::getline(iss, field, '\t')) {
                    fieldsInLine.push_back(field);
                }
                if(fieldsInLine.size() != 12)
                    continue;
                point.index =  index++;
                point.lon =  std::stod(fieldsInLine[1]);
                point.lat =  std::stod(fieldsInLine[2]);
                point.global_x =  std::stod(fieldsInLine[3]);
                point.global_y =  std::stod(fieldsInLine[4]);
                point.speed_limit =  std::stof(fieldsInLine[5]);
                point.lane_num =  std::stoi(fieldsInLine[6]);
                point.lane_index =  std::stoi(fieldsInLine[7]);
                point.left_bound =  std::stof(fieldsInLine[10]);
                point.right_bound =  std::stof(fieldsInLine[11]);
                wps_points.push_back(point);
            }
        }
        wps_file_.close();
    }
    return parallelogram_shape;
}

void Viewer2DWidget::EditWPSPointTriggered(const STRoadPoint &point,SetofMapInfo &set_of_map_info)
{
    std::map<int, SetofMapInfo> map_info;
    map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();
    SetSetofMapToMapInfo(set_of_map_info,map_info);
}

void Viewer2DWidget::drawPoint(QPainter *painter, const FPoint *pt, int r) {
  QPointF qpf;
  if (pt != NULL) {
    qpf = ToDisplayCo(*pt);
    painter->drawEllipse(qpf, r, r);
  }
}

vector<QPointF> Viewer2DWidget::drawPoints(QPainter *painter,
                                     const vector<FPoint> &pts, int r) {
  vector<QPointF> qpfs;
  QPointF qpf;
  int n = pts.size();
  for (int i = 0; i < n; i++) {
    qpf = ToDisplayCo(pts[i]);
    painter->drawEllipse(qpf, r, r);
    qpfs.push_back(qpf);
  }
  return qpfs;
}

void Viewer2DWidget::drawCurvef(QPainter *painter, vector<FPoint> pts) {
  int n = pts.size();
  vector<QPointF> qpfs;
  QPointF qpf;
  for (int i = 0; i < n; i++) {
    qpf = ToDisplayCo(pts[i]);
    qpfs.push_back(qpf);
  }
  drawCurve(painter, qpfs);
}



void Viewer2DWidget::drawCurve3(QPainter *painter, double *d, double len) {
  float gap = 2;
  int n = len / gap;
  float x, y;
  vector<FPoint> vc;
  for (int j = 0; j < n; j++) {
    y = j * gap;
    x = d[3] * y * y * y + d[2] * y * y + d[1] * y + d[0];
    vc.push_back(FPoint(x, y));
  }
  drawCurvef(painter, vc);
}


void Viewer2DWidget::paintEvent(QPaintEvent *event) {
    QPainter painter(this);

    QColor color(170, 51, 187);
    painter.setBrush(color);
    painter.drawRect(this->rect());

    painter.setRenderHint(QPainter::Antialiasing);
    drawTrakingPoints(&painter);
    drawOriginPoint(&painter);
    drawCar(&painter);
    paint(&painter, event);
    DrawMeasure(&painter);

    if((mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS)||(mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS))
    {
        if((press_flag_ != -1)&&(press_point_.index != -1))
        {
            FPoint point(press_point_.global_x,press_point_.global_y);
            painter.setPen(SET_2D_COLOR(ColorType::PURPLE));
            painter.setBrush(SET_2D_COLOR(ColorType::PURPLE));
            if(mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS)
                drawPoint(&painter,&point,5);
            else if((mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS))
                drawPoint(&painter,&point,3);

        }

        if((pick_type_ == 1 || pick_type_ == 2)&&(pick_point_.index != -1))
        {

            FPoint point(pick_point_.global_x,pick_point_.global_y);
            //wps
            painter.setPen(SET_2D_COLOR(ColorType::RED));
            painter.setBrush(SET_2D_COLOR(ColorType::RED));
            drawPoint(&painter,&point,3);

        }

    }
}

// 鼠标按下事件
void Viewer2DWidget::mousePressEvent(QMouseEvent *event) {
  // 如果是鼠标左键按下   改变指针形状，并且存储当前指针位置与窗口位置的差值。
    // 这里必须使用buttons()
    float fx, fy;
    ToRealCo(event->x(), event->y(), fx, fy);
    mousePos_.x = fx;
    mousePos_.y = fy;

    if(measurement_point_flag_ == MouseTaskType::MEASUREMENT_POINTS)
    {
        if (event->buttons() & Qt::LeftButton) {
          // 这里必须使用buttons()
          float fx, fy;
          FPoint point;
          ToRealCo(event->x(), event->y(), fx, fy);
          point.x = fx;
          point.y = fy;
          measure_points_.push_back(point);
        } else if (event->buttons() &
                   Qt::RightButton) {  //如果点击右键则清除显示的测距信息
          measure_points_.clear();
        }
    }


    if(initialization_ == false)
        return;
    if((mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS)||(mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS))
    {
        SetofMapInfo select_map_info;
        std::map<int, SetofMapInfo> map_info;
        STRoadPoint point_info_l;
        STRoadPoint best_point;
        point_info_l.global_x = mousePos_.x;
        point_info_l.global_y = mousePos_.y;
        GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(point_info_l.global_x,point_info_l.global_y,&point_info_l.lat,&point_info_l.lon);
        int pick_point_index = 0;
        map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();

        select_map_info = FindBestMatchMapInfoFromPoint(map_info,point_info_l, false);
        if(mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS)
        {
            best_point = FindBestMatchPointFromPoint(select_map_info.wps_points,point_info_l,pick_point_index,false);
            float dis = std::hypot(best_point.global_x - point_info_l.global_x,best_point.global_y - point_info_l.global_y);
            if(dis < 0.3)
            {
                press_point_ = best_point;
                press_set_of_map_info_ = select_map_info;
                press_index_ = pick_point_index;
                press_flag_ = 1;
                emit DisplayWPSPointInfoSignal(press_point_,select_map_info);
            }
        }

        if(mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS)
        {
            best_point = FindBestMatchPointFromPoint(select_map_info.gps_points,point_info_l,pick_point_index,false);
            float dis = std::hypot(best_point.global_x - point_info_l.global_x,best_point.global_y - point_info_l.global_y);
            if(dis < 0.2)
            {
                press_point_ = best_point;
                press_set_of_map_info_ = select_map_info;
                press_index_ = pick_point_index;
                press_flag_ = 1;
            }
        }
    }
}

// 鼠标释放事件
void Viewer2DWidget::mouseReleaseEvent(QMouseEvent *event)  //释放
{
  if (event->button() == Qt::LeftButton) {
      press_flag_ = 0;
  }
}
// 鼠标双击事件
void Viewer2DWidget::mouseDoubleClickEvent(QMouseEvent *event)  //双击
{
    // 这里必须使用buttons()
    float fx, fy;
    ToRealCo(event->x(), event->y(), fx, fy);
    mousePos_.x = fx;
    mousePos_.y = fy;
    if(initialization_ == false)
        return;
    if(event->button() == Qt::LeftButton)
    {
        SetofMapInfo select_map_info;
        std::map<int, SetofMapInfo> map_info;
        STRoadPoint point_info_l;
        STRoadPoint best_point;
        STRoadPoint prev_point;
        STRoadPoint post_point;
        float prev_dis;
        float post_dis;
        float prev_index;
        float post_index;
        int insert_index;
        point_info_l.global_x = mousePos_.x;
        point_info_l.global_y = mousePos_.y;
        GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(point_info_l.global_x,point_info_l.global_y,&point_info_l.lat,&point_info_l.lon);
        int pick_point_index = 0;
        map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();
        select_map_info = FindBestMatchMapInfoFromPoint(map_info,point_info_l, false);

        switch (mouse_target_type_){
        case MouseTaskType::EDIT_WPS_POINTS:
            best_point = FindBestMatchPointFromPoint(select_map_info.wps_points,point_info_l,pick_point_index,false);
            prev_index = (pick_point_index - 1)<0?0:pick_point_index - 1;
            post_index = (pick_point_index + 1)>(select_map_info.wps_points.size()-1)?select_map_info.wps_points.size()-1:pick_point_index + 1;
            prev_dis = std::hypot(select_map_info.wps_points[prev_index].global_x - point_info_l.global_x,select_map_info.wps_points[prev_index].global_y - point_info_l.global_y);
            post_dis = std::hypot(select_map_info.wps_points[post_index].global_x - point_info_l.global_x,select_map_info.wps_points[post_index].global_y - point_info_l.global_y);
            insert_index = prev_dis > post_dis?post_index:prev_index+1;
            select_map_info.wps_points.insert(select_map_info.wps_points.begin()+insert_index,point_info_l);
            SetSetofMapToMapInfo(select_map_info,map_info);
            break;
        case MouseTaskType::EDIT_GPS_POINTS:
            best_point = FindBestMatchPointFromPoint(select_map_info.gps_points,point_info_l,pick_point_index,false);
            prev_index = (pick_point_index - 1)<0?0:pick_point_index - 1;
            post_index = (pick_point_index + 1)>(select_map_info.gps_points.size()-1)?select_map_info.gps_points.size()-1:pick_point_index + 1;
            prev_dis = std::hypot(select_map_info.gps_points[prev_index].global_x - point_info_l.global_x,select_map_info.gps_points[prev_index].global_y - point_info_l.global_y);
            post_dis = std::hypot(select_map_info.gps_points[post_index].global_x - point_info_l.global_x,select_map_info.gps_points[post_index].global_y - point_info_l.global_y);
            insert_index = prev_dis > post_dis?post_index:prev_index+1;
            select_map_info.gps_points.insert(select_map_info.gps_points.begin()+insert_index,point_info_l);
            SetSetofMapToMapInfo(select_map_info,map_info);
            break;
        default:
            break;
        }
    }

}


// 鼠标移动事件
// 默认情况下，触发事件需要点击一下，才能触发。可设置为自动触发:setMouseTracking(true);
void Viewer2DWidget::mouseMoveEvent(QMouseEvent *event)  //移动
{
  // 这里必须使用buttons()
  float fx, fy;
  ToRealCo(event->x(), event->y(), fx, fy);
  mousePos_.x = fx;
  mousePos_.y = fy;

  if((mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS) || (mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS))
  {
  }else if (event->buttons() & Qt::LeftButton) {  //进行的按位与
      if (oMousPos.x() > 0) {
        OX += event->x() - oMousPos.x();
        OY += event->y() - oMousPos.y();
        oxRate = OX / width();
        oyRate = OY / height();
      }

      oMousPos.setX(event->x());
      oMousPos.setY(event->y());
    } else {
      oMousPos.setX(IVD_OMP);
    }

  if(initialization_ == false)
      return;
  if((mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS) || (mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS))
  {
      SetofMapInfo select_map_info;
      std::map<int, SetofMapInfo> map_info;
      STRoadPoint point_info_l;
      STRoadPoint best_point;
      point_info_l.global_x = mousePos_.x;
      point_info_l.global_y = mousePos_.y;
      GPSCoordinateTool::Instance()->gps_linearize_to_lat_lon(point_info_l.global_x,point_info_l.global_y,&point_info_l.lat,&point_info_l.lon);
      int pick_point_index = 0;
      map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();

      select_map_info = FindBestMatchMapInfoFromPoint(map_info,point_info_l, false);

      if((mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS))
      {
          best_point = FindBestMatchPointFromPoint(select_map_info.gps_points,point_info_l,pick_point_index,false);
          float dis = std::hypot(best_point.global_x - point_info_l.global_x,best_point.global_y - point_info_l.global_y);
          if(dis < 0.2)
          {
              pick_type_ = 2;
              pick_point_ = best_point;
          }else{
              pick_type_ = 0;
          }

          if(press_flag_ == 1)
          {
              STRoadPoint point = press_set_of_map_info_.gps_points[press_index_];
              point.lat = point_info_l.lat;
              point.lon = point_info_l.lon;
              point.global_x = point_info_l.global_x;
              point.global_y = point_info_l.global_y;
              press_set_of_map_info_.gps_points[press_index_] = point;
              press_point_ = point;
              SetSetofMapToMapInfo(press_set_of_map_info_,map_info);
          }
      }

      if(mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS)
      {
          best_point = FindBestMatchPointFromPoint(select_map_info.wps_points,point_info_l,pick_point_index,false);
          float dis = std::hypot(best_point.global_x - point_info_l.global_x,best_point.global_y - point_info_l.global_y);
          if(dis < 0.3)
          {
              pick_type_ = 1;
              pick_point_ = best_point;
          }else{
              pick_type_ = 0;
          }

          if(press_flag_ == 1)
          {
              STRoadPoint point = press_set_of_map_info_.wps_points[press_index_];
              point.lat = point_info_l.lat;
              point.lon = point_info_l.lon;
              point.global_x = point_info_l.global_x;
              point.global_y = point_info_l.global_y;
              press_set_of_map_info_.wps_points[press_index_] = point;
              press_point_ = point;
              SetSetofMapToMapInfo(press_set_of_map_info_,map_info);
              emit DisplayWPSPointInfoSignal(press_point_,press_set_of_map_info_);
          }
      }
  }
}

//修改一条路径更新处理
bool Viewer2DWidget::SetSetofMapToMapInfo(SetofMapInfo &set_of_map,std::map<int, SetofMapInfo> &map_info)
{
    for(int i=0;i<set_of_map.gps_points.size();i++)
    {
        set_of_map.gps_points.at(i).index = i;
    }
    for(int i=0;i<set_of_map.wps_points.size();i++)
    {
        set_of_map.wps_points.at(i).index = i;
    }
    map_info[set_of_map.index] = set_of_map;

    for(auto set_of_map:map_info)
    {
        if(set_of_map.second.gps_points.size() >= 2)
        {
            double dx = set_of_map.second.gps_points.at(1).global_x - set_of_map.second.gps_points.at(0).global_x;
            double dy = set_of_map.second.gps_points.at(1).global_y - set_of_map.second.gps_points.at(0).global_y;
            set_of_map.second.gps_points.at(0).yaw = vehicle::tools::cast_from_0_to_2PI_Angle((std::atan2(dy, dx)));
            set_of_map.second.gps_points.at(0).theta = vehicle::tools::cast_from_0_to_2PI_Angle(M_PI/2 - set_of_map.second.gps_points.at(0).yaw);
            for(int i=1;i<set_of_map.second.gps_points.size();i++)
            {
                double dx = set_of_map.second.gps_points.at(i).global_x - set_of_map.second.gps_points.at(i-1).global_x;;
                double dy = set_of_map.second.gps_points.at(i).global_y - set_of_map.second.gps_points.at(i-1).global_y;
                set_of_map.second.gps_points.at(i).yaw = vehicle::tools::cast_from_0_to_2PI_Angle((std::atan2(dy, dx)));
                set_of_map.second.gps_points.at(i).theta = vehicle::tools::cast_from_0_to_2PI_Angle(M_PI/2 - set_of_map.second.gps_points.at(i).yaw);
            }
        }
    }

    vehicle::map::DataProviderMapInfoPoints::Instance()->Update(map_info);

}



// 滚轮事件
void Viewer2DWidget::wheelEvent(QWheelEvent *event)  //滑轮
{
  float fx, fy, dx, dy;

  ToRealCo(event->x(), event->y(), fx, fy);
  if (event->delta() > 0)
    dRate *= 1.2;
  else if (dRate >= 1.2)
    dRate /= 1.2;
  dRate = fmin(fmax(dRate,0.001),160);
  ToDisplayCo(fx, fy, dx, dy);
  OX += event->x() - dx;
  OY += event->y() - dy;
}

void Viewer2DWidget::drawCar(QPainter *painter)
{
    string network_time;
    STRoadPoint point_info_l = vehicle::map::DataProviderEgoPosition::Instance()->ego_position();
    vehicle::tools::GetTime(&network_time);
    main_window_->timeLable->setText("Time: "+QString::fromStdString(network_time));
    main_window_->locationLabel->setText(tr("Location:<%1, %2| %3>").arg(point_info_l.lon, 0, 'f', 6).arg(point_info_l.lat, 0, 'f', 6).arg(point_info_l.location_state));



    painter->setPen(SET_2D_COLOR(ColorType::WHITE));
    painter->setBrush(SET_2D_COLOR(ColorType::WHITE));
    FPoint ego_point(point_info_l.global_x,point_info_l.global_y);
    drawPoint(painter,&ego_point,7);
}

void Viewer2DWidget::paint(QPainter *painter, QPaintEvent *event) {
    drawMap(painter);
}

void Viewer2DWidget::drawTrakingPoints(QPainter *painter)
{
    if(main_window_->traking_gps_flag_ == MouseTaskType::TRAKING_GPS_POINTS)
    {
        std::vector<STRoadPoint> traking_points_array;
        traking_points_->ToArrayR(traking_points_array);
        if(traking_points_array.size() > 0)
        {
            for(auto point: traking_points_array)
            {
                painter->setBrush(SET_2D_COLOR(ColorType::RED));
                FPoint ego_point(point.global_x,point.global_y);
                drawPoint(painter,&ego_point,3);
            }
        }
    }
}

void Viewer2DWidget::drawBoundaryBox(QPainter *painter, SetofMapInfo &select_map_info)
{
    QPointF qpf0,qpf1;
    painter->setPen(SET_2D_COLOR(ColorType::RED));
    std::vector<Vec2d> points = select_map_info.boundary_box.GetAllCorners();
    if(points.size()>1)
    {
        for(int i=0;i<points.size()-1;i++)
        {
            qpf0 = ToDisplayCo(FPoint(points.at(i).x(),points.at(i).y()));
            qpf1 = ToDisplayCo(FPoint(points.at(i+1).x(),points.at(i+1).y()));
            painter->drawLine(qpf0,qpf1);
        }
        qpf0 = ToDisplayCo(FPoint(points.at(points.size()-1).x(),points.at(points.size()-1).y()));
        qpf1 = ToDisplayCo(FPoint(points.at(0).x(),points.at(0).y()));
        painter->drawLine(qpf0,qpf1);
    }
}

void Viewer2DWidget::drawSetofMapInfo(QPainter *painter, SetofMapInfo &select_map_info)
{
    vector<FPoint> map_points;
    vector<FPoint> wps_points;
    map_points.clear();
    wps_points.clear();
    for(int i=0;i<select_map_info.gps_points.size();i++)
    {
        FPoint point(select_map_info.gps_points.at(i).global_x,select_map_info.gps_points.at(i).global_y);
        map_points.push_back(point);
    }

    for(int i=0;i<select_map_info.wps_points.size();i++)
    {
        FPoint point(select_map_info.wps_points.at(i).global_x,select_map_info.wps_points.at(i).global_y);
        wps_points.push_back(point);
    }

    painter->setBrush(SET_2D_COLOR(ColorType::GREEN));
    drawPoints(painter,map_points,3);
    painter->setPen(SET_2D_COLOR(ColorType::GREEN));
    drawCurvef(painter,map_points);

    painter->setBrush(SET_2D_COLOR(ColorType::YELLOW));
    drawPoints(painter,wps_points,5);
//    drawLines(painter,wps_points,1,ColorType::YELLOW);


}

void Viewer2DWidget::drawPublishMapInfo(QPainter *painter)
{
    lcmtypes::map_info_t map_info =  vehicle::map::DataProviderMapInfo::Instance()->MapInfo();

    vector<FPoint> map_points;
    map_points.clear();
    for(int i=0;i<map_info.frontLaneInfo[1].points.size();i++)
    {
        FPoint point(map_info.frontLaneInfo[1].points.at(i).globalX,map_info.frontLaneInfo[1].points.at(i).globalY);
        map_points.push_back(point);
    }

    if(map_points.size() > 0)
    {
//        painter->setBrush(SET_2D_COLOR(ColorType::DEEP_GREEN));
//        drawPoints(painter,map_points,3);
        QPen pen;
        pen.setColor(SET_2D_COLOR(ColorType::RED));
        pen.setWidth(5);
        painter->setPen(pen);
        drawCurvef(painter,map_points);
    }

    for(int i=0;i<map_info.boundaryCount;i++)
    {
        map_points.clear();
        for(int j=0;j<map_info.boundary.at(i).points.size();j++)
        {
            FPoint point(map_info.boundary.at(i).points.at(j).globalX,map_info.boundary.at(i).points.at(j).globalY);
            map_points.push_back(point);
        }
        if(map_points.size() > 0)
        {

            QPen pen;
            pen.setColor(SET_2D_COLOR(ColorType::MAGENTA));
            pen.setWidth(5);
            painter->setPen(pen);
            drawPoints(painter,map_points,4);
       //     drawCurvef(painter,map_points);
        }
    }
}



void Viewer2DWidget::drawMap(QPainter *painter)
{
    if((mouse_target_type_ == MouseTaskType::EDIT_GPS_POINTS)||(mouse_target_type_ == MouseTaskType::EDIT_WPS_POINTS))
    {
        std::map<int, SetofMapInfo> map_info;
        map_info = vehicle::map::DataProviderMapInfoPoints::Instance()->map_info_points();
        for(auto set_of_map:map_info)
        {
            drawSetofMapInfo(painter,set_of_map.second);
            drawBoundaryBox(painter,set_of_map.second);
        }

    }else if(mouse_target_type_ == MouseTaskType::RECORD_MAP_POINTS){
        std::vector<STRoadPoint> wps_points = main_window_->RecordWPSPointsInfo();
        std::vector<STRoadPoint> gps_points = main_window_->RecordGPSPointsInfo();
        vector<FPoint> wps_Fpoints;
        vector<FPoint> gps_Fpoints;
        for(auto point:wps_points)
        {
            wps_Fpoints.push_back(FPoint(point.global_x,point.global_y));
        }

        for(auto point:gps_points)
        {
            gps_Fpoints.push_back(FPoint(point.global_x,point.global_y));
        }
        if(gps_Fpoints.size() > 0)
        {
            painter->setBrush(SET_2D_COLOR(ColorType::GREEN));
            drawPoints(painter,gps_Fpoints,5);
        }
        if(wps_Fpoints.size() > 0)
        {
            painter->setBrush(SET_2D_COLOR(ColorType::YELLOW));
            drawPoints(painter,wps_Fpoints,7);
        }
    }else{
        SetofMapInfo select_map_info = vehicle::map::DataProviderSetofMapInfo::Instance()->set_of_map_info();
        drawSetofMapInfo(painter,select_map_info);
        drawBoundaryBox(painter,select_map_info);
        FPoint next_wps_point(next_wps_point_.global_x,next_wps_point_.global_y);
        painter->setBrush(QColor(0,0,0));
        painter->setPen(QColor(0,0,0));
        drawPoint(painter,&next_wps_point,8);
        drawPublishMapInfo(painter);
    }
}

void Viewer2DWidget::DrawMeasure(QPainter *painter)
{
    if (measure_points_.size() > 0) {
        int k;
        for (k = 0; k < measure_points_.size(); ++k) {
            drawCircleOb(painter,"",measure_points_[k].x, measure_points_[k].y,0.02,ColorType::AQUA);
        }
        drawCircleOb(painter,"",mousePos_.x, mousePos_.y,0.02,ColorType::AQUA);

        QPointF qpf0,qpf1;
        int i;
        for (i = 1; i < measure_points_.size(); i++) {
          painter->setPen(SET_2D_COLOR(ColorType::AQUA));
          qpf0 = ToDisplayCo(measure_points_[i-1]);
          qpf1 = ToDisplayCo(measure_points_[i]);
          painter->drawLine(qpf0,qpf1);
          FPoint dis =  measure_points_[i].operator-(measure_points_[i-1]);
          QString qstr = QString::number(std::sqrt(dis.y * dis.y + dis.x * dis.x), 'f', 2);
          QFont font(qstr, 6); // 设置字体为 Arial，大小为 20
          painter->setFont(font);
          painter->drawText(qpf1.x(), qpf1.y() - 4, qstr);
        }

        qpf0 = ToDisplayCo(measure_points_[i-1]);
        qpf1 = ToDisplayCo(mousePos_);
        painter->drawLine(qpf0,qpf1);
        FPoint dis =  mousePos_.operator-(measure_points_[i-1]);
        QString qstr = QString::number(std::sqrt(dis.y * dis.y + dis.x * dis.x), 'f', 2);
        QFont font(qstr, 6); // 设置字体为 Arial，大小为 20
        painter->setFont(font);
        painter->setPen(SET_2D_COLOR(ColorType::AQUA));
        painter->drawText(qpf1.x(), qpf1.y() - 4, qstr);
    }
}

void Viewer2DWidget::drawOriginPoint(QPainter *painter) {
    float dx1,dy1;
    ToDisplayCo(0, 0, dx1, dy1);

    // 定义坐标系参数
    int axisLength = 10*dRate;
    int arrowSize = 0.8*dRate;
    QPoint origin(dx1,dy1);  // 坐标原点
    // 绘制坐标轴
    painter->setPen(SET_2D_COLOR(ColorType::BLUE));    //画笔
    painter->setBrush(SET_2D_COLOR(ColorType::BLUE));   // 画刷
    painter->drawLine(origin, origin + QPoint(0, -axisLength));  // Y轴
    drawArrow(painter, origin, origin + QPoint(0, -axisLength), arrowSize);  // Y轴箭头

    painter->setPen(SET_2D_COLOR(ColorType::RED));    //画笔
    painter->setBrush(SET_2D_COLOR(ColorType::RED));   // 画刷
    painter->drawLine(origin, origin + QPoint(axisLength, 0));   // X轴
    drawArrow(painter, origin, origin + QPoint(axisLength, 0), arrowSize, false);   // X轴箭头
}


void Viewer2DWidget::drawArrow(QPainter *painter, const QPoint &start, const QPoint &end, int arrowSize, bool isYAxis) {
        // 绘制箭头
        QPolygon arrow;
        if (isYAxis) {
            arrow << end << end + QPoint(-arrowSize, arrowSize) << end + QPoint(arrowSize, arrowSize);
        } else {
            arrow << end << end + QPoint(-arrowSize, -arrowSize) << end + QPoint(-arrowSize, arrowSize);
        }
        painter->drawPolygon(arrow);

        // 绘制箭头轴线
        painter->drawLine(start, end);
    }


void Viewer2DWidget::drawOb(QPainter *painter, float x1, float x2, float y, float v) {
  if (fabs(y) < 200) {
    QString qstr;
    float dx1, dy1, dx2, dy2;
    ToDisplayCo(x1, y, dx1, dy1);
    ToDisplayCo(x2, y, dx2, dy2);
    painter->setPen(obPen);
    painter->drawLine(dx1, dy1, dx2, dy2);
    qstr = QString("%1").arg(fabs(y), 0, 'f', 1);
    painter->setPen(QColor(250, 50, 150));
    painter->drawText((dx1 + dx2) / 2 - (qstr.length() - 1) * 5, dy1 - 4, qstr);
    qstr = QString("%1").arg(v, 0, 'f', 1);
    painter->setPen(QColor(250, 50, 150));
    painter->drawText((dx1 + dx2) / 2 - (qstr.length() - 1) * 5, dy1 + 13, qstr);
  }
}

void Viewer2DWidget::drawOb(QPainter *painter,float x,float y,float width,float height,int color)
{
    float dx1 = 0, dy1 = 0, dx2, dy2;
    QString qstr;
    // 定义矩形的左上角和右下角坐标
    ToDisplayCo(x-width/2.0, y + height/2.0, dx1, dy1);
    QPoint topLeft(dx1, dy1);
    ToDisplayCo(x+width/2.0, y - height/2.0,dx2,dy2);
    QPoint bottomRight(dx2,dy2);
    QRect rectangle(topLeft, bottomRight);
    painter->setBrush(QColor(255, 0, 0)); // 红色
    // 绘制矩形
    qstr = QString("XY:[%1,%2]").arg(fabs(x), 0, 'f', 1).arg(fabs(y), 0, 'f', 1);

    painter->setPen(QColor(250, 50, 150));
    painter->drawText((dx1 + dx2) / 2 - (qstr.length() - 1) *3, dy1 - 4, qstr);
    painter->drawRect(rectangle);
}

void Viewer2DWidget::drawLine(QPainter *painter,FPoint p1,FPoint p2,int width,int color)
{
    painter->setPen(SET_2D_COLOR(ColorType::YELLOW));
    float dx1, dy1, dx2, dy2;
    ToDisplayCo(p1.x, p1.y, dx1, dy1);
    ToDisplayCo(p2.x, p2.y, dx2, dy2);
    painter->drawLine(dx1, dy1,dx2, dy2);
}

void Viewer2DWidget::drawLines(QPainter *painter,std::vector<FPoint> p,int width,int color)
{
    QVector<QPointF> pfs;
    pfs.clear();
    for(int i=1;i<p.size();i++)
    {
        drawLine(painter,p.at(i-1),p.at(i),10,color);
    }
}


void Viewer2DWidget::drawCircleOb(QPainter *painter,std::string str,float x,float y,float real_r,int color)
{
    float dx1 = 0, dy1 = 0, dx2, dy2, dx3, dy3;
    QString qstr;

    ToDisplayCo(x-real_r,y,dx1,dy1);
    ToDisplayCo(x+real_r,y,dx2,dy2);
    ToDisplayCo(x, y, dx3, dy3);
    double distance = std::sqrt((dx2-dx1)*(dx2-dx1)+(dy2-dy1)*(dy2-dy1));
    painter->setBrush(SET_2D_COLOR(color));

    if(!str.empty())
    {
        qstr = QString::fromStdString(str);
        QFont font(qstr, 6); // 设置字体为 Arial，大小为 20
        painter->setFont(font);
        painter->setPen(QColor(0, 0, 0));
        painter->drawText((dx1 + dx2) / 2 - (qstr.length() - 1) *3, dy1 - 4, qstr);
    }
    painter->drawEllipse(dx3-distance/2.0,dy3-distance/2.0,distance,distance);
}


void Viewer2DWidget::resizeEvent(QResizeEvent *size) { 
  SetOXY(width(), height()); 
}

void Viewer2DWidget::SetOXY(float w, float h) {
  OX = w * oxRate;
  OY = h * oyRate;
}
